#! /usr/bin/python3
# 摄像头捕捉物体颜色
import cv2
import numpy as np
import serial


def rec(serial1):  # 串口接收
    while True:
        data1 = serial1.read_all().decode()  # str
        if data1 == '':
            break
        else:
            break
    return data1


# 求轮廓面积
def areaCal(contour):
    area = 0
    for j in range(len(contour)):
        area += cv2.contourArea(contour[j])
    return area


ser = serial.Serial('COM3', 115200, timeout=0.5)


# ser.write(bytes(a[0], encoding='utf-8'))  # 十六进制发送数据


def detect():
    cap = cv2.VideoCapture(0)

    while True:
        ret, src = cap.read()
        # 灰度转换
        gray = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY)
        # 检测
        qrcoder = cv2.QRCodeDetector()
        # 解码
        codeinfo, points, straight_qrcode = qrcoder.detectAndDecode(gray)
        print(straight_qrcode)
        result = np.copy(src)
        # 描绘轮廓
        # cv2.drawContours(result, [np.int32(points)], 0, (0, 0, 255), 2)
        # print("qrcode : %s" % codeinfo)
        # print(codeinfo)
        # 添加文字
        cv2.putText(result, "qrcode:" + str(codeinfo), (0, 20), cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 0, 0), 1)
        cv2.imshow("result", result)
        cv2.waitKey(1)
        edema = codeinfo
        if straight_qrcode is not None:
            cv2.destroyAllWindows()
            break
    return edema


def manifesto():  # 读取物块颜色
    r = 0
    g = 0
    b = 0
    r1 = 0
    g1 = 0
    b1 = 0
    r2 = 0
    g2 = 0
    b2 = 0
    # cam = cv2.VideoCapture(0, cv2.CAP_DSHOW)  # 打开摄像头

    while cam.isOpened():  # 数据库是否被打开
        ret, frame1 = cam.read()  # 读取摄像头数据
        if dicier == 1:
            frame = frame1[200:380, 50:250]  # y;x
            frame2 = frame1[200:380, 220:420]
            frame3 = frame1[200:380, 350:600]
        else:
            frame = frame1[200:400, 100:300]  # y;x
            frame2 = frame1[200:400, 270:450]
            frame3 = frame1[200:400, 430:580]
        if ret:  # 如果有数据
            if frame is not None:  # 如果不是空数据
                gs_frame = cv2.GaussianBlur(frame, (5, 5), 0)  # 高斯模糊
                hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)  # 转化成HSV图像
                erode_hsv = cv2.erode(hsv, None, iterations=2)  # 腐蚀粗地变细
                in_range_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'],
                                           color_dist[ball_color]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                in_range_hsv1 = cv2.inRange(erode_hsv, color_dist[ball_color2]['Lower'],
                                            color_dist[ball_color2]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                in_range_hsv2 = cv2.inRange(erode_hsv, color_dist[ball_color1]['Lower'],
                                            color_dist[ball_color1]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                cents, hierarchy = cv2.findContours(in_range_hsv.copy(), cv2.RETR_EXTERNAL,
                                                    cv2.CHAIN_APPROX_SIMPLE)
                cents1, hierarchy1 = cv2.findContours(in_range_hsv1.copy(), cv2.RETR_EXTERNAL,
                                                      cv2.CHAIN_APPROX_SIMPLE)
                cents2, hierarchy2 = cv2.findContours(in_range_hsv2.copy(), cv2.RETR_EXTERNAL,
                                                      cv2.CHAIN_APPROX_SIMPLE)
                # 要寻找轮廓的图像 只能传入二值图像，不是灰度图像。cv2.findContours()函数返回两个值，一个是轮廓本身contours，还有一个是每条轮廓对应的属性hierarchy。
                # cv2.RETR_EXTERNAL表示只检测外轮廓。
                # cv2.RETR_LIST检测的轮廓不建立等级关系。
                # cv2.RETR_CCOMP建立两个等级的轮廓，上面的一层为外边界，里面的一层为内孔的边界信息。如果内孔内还有一个连通物体，这个物体的边界也在顶层。
                # cv2.RETR_TREE建立一个等级树结构的轮廓
                # cv2.CHAIN_APPROX_NONE存储所有的轮廓点，相邻的两个点的像素位置差不超过1，即max（abs（x1 - x2），abs（y2 - y1）） == 1　　
                # cv2.CHAIN_APPROX_SIMPLE压缩水平方向，垂直方向，对角线方向的元素，只保留该方向的终点坐标，例如一个矩形轮廓只需4个点来保存轮廓信息

                try:
                    c = max(cents, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                    rect = cv2.minAreaRect(c)  # 绘制出该区域的最小外接矩形
                    box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                    r = areaCal(cents)
                    if b < r and g < r:
                        yandex[0] = 'r'
                        cv2.drawContours(frame, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                    # print("红色")
                    # print(a)
                except ValueError:
                    # print('未找到红')
                    pass
                try:
                    c1 = max(cents1, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                    rect = cv2.minAreaRect(c1)  # 绘制出该区域的最小外接矩形
                    box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                    b = areaCal(cents1)
                    if b > r and g < b:
                        yandex[0] = 'b'
                        cv2.drawContours(frame, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                    # print("蓝色")
                    # print(a)
                except ValueError:
                    # print('未找到蓝')
                    pass
                try:
                    c2 = max(cents2, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                    rect = cv2.minAreaRect(c2)  # 绘制出该区域的最小外接矩形
                    box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                    g = areaCal(cents2)
                    if g > r and g > b:
                        yandex[0] = 'g'
                        cv2.drawContours(frame, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                    # print("绿色")
                    # print(a)
                except ValueError:
                    # print('未找到绿')
                    pass
                # bgr参数一：frame目标图像
                # 参数二：[np.int0(box)]轮廓本身，在Python中是一个list
                # 参数三：-1指定绘制轮廓list中的哪条轮廓，如果是-1，则绘制其中的所有轮廓。
                # 参数四：(0, 255, 255)轮廓颜色BGR
                # 参数五：2廓线的宽度
                if frame2 is not None:  # 如果不是空数据
                    gs_frame = cv2.GaussianBlur(frame2, (5, 5), 0)  # 高斯模糊
                    hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)  # 转化成HSV图像
                    erode_hsv = cv2.erode(hsv, None, iterations=2)  # 腐蚀 粗地变细
                    in_range_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'],
                                               color_dist[ball_color]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                    in_range_hsv1 = cv2.inRange(erode_hsv, color_dist[ball_color2]['Lower'],
                                                color_dist[ball_color2]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                    in_range_hsv2 = cv2.inRange(erode_hsv, color_dist[ball_color1]['Lower'],
                                                color_dist[ball_color1]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                    cents, hierarchy = cv2.findContours(in_range_hsv.copy(), cv2.RETR_EXTERNAL,
                                                        cv2.CHAIN_APPROX_SIMPLE)
                    cents1, hierarchy1 = cv2.findContours(in_range_hsv1.copy(), cv2.RETR_EXTERNAL,
                                                          cv2.CHAIN_APPROX_SIMPLE)
                    cents2, hierarchy2 = cv2.findContours(in_range_hsv2.copy(), cv2.RETR_EXTERNAL,
                                                          cv2.CHAIN_APPROX_SIMPLE)
                    # 要寻找轮廓的图像 只能传入二值图像，不是灰度图像。cv2.findContours(
                    # )函数返回两个值，一个是轮廓本身contours，还有一个是每条轮廓对应的属性hierarchy。 cv2.RETR_EXTERNAL表示只检测外轮廓。
                    # cv2.RETR_LIST检测的轮廓不建立等级关系。
                    # cv2.RETR_CCOMP建立两个等级的轮廓，上面的一层为外边界，里面的一层为内孔的边界信息。如果内孔内还有一个连通物体，这个物体的边界也在顶层。
                    # cv2.RETR_TREE建立一个等级树结构的轮廓 cv2.CHAIN_APPROX_NONE存储所有的轮廓点，相邻的两个点的像素位置差不超过1，即max（abs（x1 -
                    # x2），abs（y2 - y1）） == 1　　
                    # cv2.CHAIN_APPROX_SIMPLE压缩水平方向，垂直方向，对角线方向的元素，只保留该方向的终点坐标，例如一个矩形轮廓只需4个点来保存轮廓信息

                    try:
                        c = max(cents, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                        rect = cv2.minAreaRect(c)  # 绘制出该区域的最小外接矩形
                        box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                        r1 = areaCal(cents)
                        if b1 < r1 and g1 < r1:
                            yandex[1] = 'r'
                            cv2.drawContours(frame2, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                        # print("红色")
                        # print(a)
                    except ValueError:
                        # print('未找到红')
                        pass
                    try:
                        c1 = max(cents1, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                        rect = cv2.minAreaRect(c1)  # 绘制出该区域的最小外接矩形
                        box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                        b1 = areaCal(cents1)
                        if b1 > r1 and g1 < b1:
                            yandex[1] = 'b'
                            cv2.drawContours(frame2, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                        # print("蓝色")
                        # print(a)
                    except ValueError:
                        # print('未找到蓝')
                        pass
                    try:
                        c2 = max(cents2, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                        rect = cv2.minAreaRect(c2)  # 绘制出该区域的最小外接矩形
                        box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                        g1 = areaCal(cents2)
                        if g1 > r1 and g1 > b1:
                            yandex[1] = 'g'
                            cv2.drawContours(frame2, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                        # print("绿色")
                        # print(a)
                    except ValueError:
                        # print('未找到绿')
                        pass
                    if frame3 is not None:  # 如果不是空数据
                        gs_frame = cv2.GaussianBlur(frame3, (5, 5), 0)  # 高斯模糊
                        hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)  # 转化成HSV图像
                        erode_hsv = cv2.erode(hsv, None, iterations=2)  # 腐蚀 粗地变细
                        in_range_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'],
                                                   color_dist[ball_color]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                        in_range_hsv1 = cv2.inRange(erode_hsv, color_dist[ball_color2]['Lower'],
                                                    color_dist[ball_color2]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                        in_range_hsv2 = cv2.inRange(erode_hsv, color_dist[ball_color1]['Lower'],
                                                    color_dist[ball_color1]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                        in_range_hsv_1 = cv2.inRange(erode_hsv, color_dist_1[ball_color_1]['Lower'],
                                                     color_dist_1[ball_color_1]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                        in_range_hsv1_1 = cv2.inRange(erode_hsv, color_dist_1[ball_color2_1]['Lower'],
                                                      color_dist_1[ball_color2_1]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                        in_range_hsv2_1 = cv2.inRange(erode_hsv, color_dist_1[ball_color1_1]['Lower'],
                                                      color_dist_1[ball_color1_1]['Upper'])  # 去除背景，阈值内的为白，阈值外的为黑
                        in_range_hsv = cv2.addWeighted(in_range_hsv, 1, in_range_hsv_1, 1, 0)
                        in_range_hsv1 = cv2.addWeighted(in_range_hsv1, 1, in_range_hsv1_1, 1, 0)
                        in_range_hsv2 = cv2.addWeighted(in_range_hsv2, 1, in_range_hsv2_1, 1, 0)

                        cents, hierarchy = cv2.findContours(in_range_hsv.copy(), cv2.RETR_EXTERNAL,
                                                            cv2.CHAIN_APPROX_SIMPLE)
                        cents1, hierarchy1 = cv2.findContours(in_range_hsv1.copy(), cv2.RETR_EXTERNAL,
                                                              cv2.CHAIN_APPROX_SIMPLE)
                        cents2, hierarchy2 = cv2.findContours(in_range_hsv2.copy(), cv2.RETR_EXTERNAL,
                                                              cv2.CHAIN_APPROX_SIMPLE)
                        # 要寻找轮廓的图像 只能传入二值图像，不是灰度图像。cv2.findContours(
                        # )函数返回两个值，一个是轮廓本身contours，还有一个是每条轮廓对应的属性hierarchy。 cv2.RETR_EXTERNAL表示只检测外轮廓。
                        # cv2.RETR_LIST检测的轮廓不建立等级关系。
                        # cv2.RETR_CCOMP建立两个等级的轮廓，上面的一层为外边界，里面的一层为内孔的边界信息。如果内孔内还有一个连通物体，这个物体的边界也在顶层。
                        # cv2.RETR_TREE建立一个等级树结构的轮廓
                        # cv2.CHAIN_APPROX_NONE存储所有的轮廓点，相邻的两个点的像素位置差不超过1，即max（abs（x1 - x2），abs（y2 - y1）） ==
                        # 1　　 cv2.CHAIN_APPROX_SIMPLE压缩水平方向，垂直方向，对角线方向的元素，只保留该方向的终点坐标，例如一个矩形轮廓只需4个点来保存轮廓信息
                        try:
                            c = max(cents, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                            rect = cv2.minAreaRect(c)  # 绘制出该区域的最小外接矩形
                            box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                            r2 = areaCal(cents)
                            if b2 < r2 and g2 < r2:
                                cv2.drawContours(frame3, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                                yandex[2] = 'r'
                            # print("红色")
                            # print(a)
                        except ValueError:
                            # print('未找到红')
                            pass
                        try:
                            c1 = max(cents1, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                            rect = cv2.minAreaRect(c1)  # 绘制出该区域的最小外接矩形
                            box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                            b2 = areaCal(cents1)
                            if b2 > r2 and g2 < b2:
                                yandex[2] = 'b'
                                cv2.drawContours(frame3, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                            # print("蓝色")
                            # print(a)
                        except ValueError:
                            # print('未找到蓝')
                            pass
                        try:
                            c2 = max(cents2, key=cv2.contourArea)  # 在边界中找出面积最大的区域
                            rect = cv2.minAreaRect(c2)  # 绘制出该区域的最小外接矩形
                            box = cv2.boxPoints(rect)  # 记录该矩形四个点的位置坐标
                            g2 = areaCal(cents2)
                            if g2 > r2 and g2 > b2:
                                yandex[2] = 'g'
                                cv2.drawContours(frame3, [np.int0(box)], -1, (255, 255, 0), 2)  # 绘制矩形
                            # print("绿色")
                            # print(a)
                        except ValueError:
                            # print('未找到绿')
                            pass
                    # bgr参数一：frame目标图像
                    # 参数二：[np.int0(box)]轮廓本身，在Python中是一个list
                    # 参数三：-1指定绘制轮廓list中的哪条轮廓，如果是-1，则绘制其中的所有轮廓。
                    # 参数四：(0, 255, 255)轮廓颜色BGR
                    # 参数五：2廓线的宽度

                    cv2.imshow('camera1', frame)
                    # cv2.moveWindow('camera1', 0, 600)
                    cv2.imshow('camera', frame1)
                    # cv2.moveWindow('camera2', 200, 0)
                    cv2.imshow('camera2', frame2)
                    # cv2.moveWindow('camera3', 400, 0)
                    cv2.imshow('camera3', frame3)
                    cv2.waitKey(1)
                    try:
                        print(yandex[0] + yandex[1] + yandex[2])

                    except KeyError:
                        pass
                # if cv2.waitKey(1) == 27:
                #     cam.release()
                #     cv2.destroyAllWindows()

                if yandex[0]:
                    return yandex

            else:
                print("无画面")
        else:
            print("无法读取摄像头！")


while True:
    data = rec(ser)  # 接收数据
    print(data)
    if 1:
        ball_color = 'red'
        ball_color1 = 'green'
        ball_color2 = 'blue'
        color_dist = {'red': {'Lower': np.array([0, 43, 46]), 'Upper': np.array([10, 255, 255])},
                      'blue': {'Lower': np.array([100, 43, 46]), 'Upper': np.array([124, 255, 255])},
                      'green': {'Lower': np.array([35, 43, 46]), 'Upper': np.array([77, 255, 255])},
                      }
        ball_color_1 = 'red1'
        ball_color1_1 = 'green1'
        ball_color2_1 = 'blue1'
        color_dist_1 = {'red1': {'Lower': np.array([156, 43, 46]), 'Upper': np.array([180, 255, 255])},
                        'blue1': {'Lower': np.array([100, 43, 46]), 'Upper': np.array([124, 255, 255])},
                        'green1': {'Lower': np.array([35, 43, 46]), 'Upper': np.array([77, 255, 255])},
                        }
        yandex = ['r', 'r', 'r']

        # cap = cv2.VideoCapture(0)  # 打开摄像头

        ewer = detect()  # 二维码读取
        if ewer[0] == '红' and ewer[2] == '蓝':
            ser.write(bytes('q', encoding='utf-8'))  # 十六进制发送数据
        elif ewer[0] == '红' and ewer[2] == '绿':
            ser.write(bytes('w', encoding='utf-8'))  # 十六进制发送数据
        elif ewer[0] == '蓝' and ewer[2] == '绿':
            ser.write(bytes('e', encoding='utf-8'))  # 十六进制发送数据
        elif ewer[0] == '蓝' and ewer[2] == '红':
            ser.write(bytes('r', encoding='utf-8'))  # 十六进制发送数据
        elif ewer[0] == '绿' and ewer[2] == '蓝':
            ser.write(bytes('t', encoding='utf-8'))  # 十六进制发送数据
        elif ewer[0] == '绿' and ewer[2] == '红':
            ser.write(bytes('y', encoding='utf-8'))  # 十六进制发送数据
        ewer_1 = [1, 2]
        dicier = 0
        # print(ewer)
        # ser.write(bytes(str(1), encoding='utf-8'))  # 十六进制发送数据
        cv2.namedWindow('camera', 0)  # 打开窗口且窗口不可调大小（1不可调，0可调）
        cv2.resizeWindow('camera', 600, 600)  # 控制窗口打开后的长宽(x,y)
        cv2.namedWindow('camera1', 0)  # 打开窗口且窗口不可调大小（1不可调，0可调）
        cv2.resizeWindow('camera1', 200, 600)  # 控制窗口打开后的长宽(x,y)
        cv2.namedWindow('camera2', 0)  # 打开窗口且窗口不可调大小（1不可调，0可调）
        cv2.resizeWindow('camera2', 200, 600)  # 控制窗口打开后的长宽(x,y)
        cv2.namedWindow('camera3', 0)  # 打开窗口且窗口不可调大小（1不可调，0可调）
        cv2.resizeWindow('camera3', 200, 600)  # 控制窗口打开后的长宽(x,y)
        cv2.moveWindow('camera', 1200, 600)
        cv2.moveWindow('camera1', 300, 0)
        cv2.moveWindow('camera2', 600, 0)
        cv2.moveWindow('camera3', 900, 0)

        cam = cv2.VideoCapture(0)  # 打开摄像头
        while True:
            manse = manifesto()  # 第一次读取色块颜色
            # 获取抓取的顺序0

            data = rec(ser)  # 接收数据
            print(data)
            if data == 'k':
                dicier = 1
                while True:
                    data = rec(ser)  # 接收数据
                    print(data)
                    if ewer[0] == '红':
                        if manse[0] == 'r':
                            ewer_1[0] = '1'
                        elif manse[1] == 'r':
                            ewer_1[0] = '2'
                        elif manse[2] == 'r':
                            ewer_1[0] = '3'
                    if ewer[0] == '绿':
                        if manse[0] == 'g':
                            ewer_1[0] = '1'
                        elif manse[1] == 'g':
                            ewer_1[0] = '2'
                        elif manse[2] == 'g':
                            ewer_1[0] = '3'
                    if ewer[0] == '蓝':
                        if manse[0] == 'b':
                            ewer_1[0] = '1'
                        elif manse[1] == 'b':
                            ewer_1[0] = '2'
                        elif manse[2] == 'b':
                            ewer_1[0] = '3'

                        # 获取抓取的顺序1
                    if ewer[2] == '红':
                        if manse[0] == 'r':
                            ewer_1[1] = '1'
                        elif manse[1] == 'r':
                            ewer_1[1] = '2'
                        elif manse[2] == 'r':
                            ewer_1[1] = '3'
                    if ewer[2] == '绿':
                        if manse[0] == 'g':
                            ewer_1[1] = '1'
                        elif manse[1] == 'g':
                            ewer_1[1] = '2'
                        elif manse[2] == 'g':
                            ewer_1[1] = '3'
                    if ewer[2] == '蓝':
                        if manse[0] == 'b':
                            ewer_1[1] = '1'
                        elif manse[1] == 'b':
                            ewer_1[1] = '2'
                        elif manse[2] == 'b':
                            ewer_1[1] = '3'
                    print('w')
                    print(ewer_1[0])
                    print(ewer_1[1])
                    for i in range(200):
                        ser.write(bytes('m', encoding='utf-8'))  # 十六进制发送数据
                        ser.write(bytes(str(ewer_1[0]), encoding='utf-8'))  # 十六进制发送数据
                        ser.write(bytes(str(ewer_1[1]), encoding='utf-8'))  # 十六进制发送数据
                    while True:
                        data = rec(ser)  # 接收数据
                        if data == 'o':
                            break
                    break
        # cv2.destroyAllWindows()
